///////////////////////////////////////////////////////////////////////////
//
// NAME
//  BlendImages.cpp -- blend together a set of overlapping images
//
// DESCRIPTION
//  This routine takes a collection of images aligned more or less horizontally
//  and stitches together a mosaic.
//
//  The images can be blended together any way you like, but I would recommend
//  using a soft halfway blend of the kind Steve presented in the first lecture.
//
//  Once you have blended the images together, you should crop the resulting
//  mosaic at the halfway points of the first and last image.  You should also
//  take out any accumulated vertical drift using an affine warp.
//  Lucas-Kanade Taylor series expansion of the registration error.
//
// SEE ALSO
//  BlendImages.h       longer description of parameters
//
// Copyright ?Richard Szeliski, 2001.  See Copyright.h for more details
// (modified for CSE455 Winter 2003 and CS4670 Fall 2012)
//
///////////////////////////////////////////////////////////////////////////

#include "ImageLib/ImageLib.h"
#include "BlendImages.h"
#include <float.h>
#include <math.h>
#include <iostream>

#define MAX(x,y) (((x) < (y)) ? (y) : (x))
#define MIN(x,y) (((x) < (y)) ? (x) : (y))

Line::Line()
{
	a = 0;
	b = 0;
	c = 0;
}

Line::Line(double x1, double y1, double x2, double y2)
{
	if (x1 == x2)
	{
		b = 0;
		a = 1;
		c = - x1;
	}
	else if (y1 == y2)
	{
		a = 0;
		b = 1;
		c = -y1;
	}
	else
	{
		a = (y1 - y2) / (x1 - x2);
		b = -1;
		c = y1 - a * x1;
	}
}

double Line::getDistance(double x, double y)
{
	return (a * x + b * y + c) / sqrt(a * a + b * b);
}

// Return the closest integer to x, rounding up
static int iround(double x) {
	if (x < 0.0) {
		return (int) (x - 0.5);
	} else {
		return (int) (x + 0.5);
	}
}

/******************* TO DO *********************
* AccumulateBlend:
*	INPUT:
*		img: a new image to be added to acc
*		acc: portion of the accumulated image where img is to be added
*      M: transformation matrix for computing a bounding box
*		blendWidth: width of the blending function (horizontal hat function;
*	    try other blending functions for extra credit)
*	OUTPUT:
*		add a weighted copy of img to the subimage specified in acc
*		the first 3 band of acc records the weighted sum of pixel colors
*		the fourth band of acc records the sum of weight
*/
static void AccumulateBlend(CByteImage& img, CFloatImage& acc, CTransform3x3 M, float blendWidth)
{
	// BEGIN TODO
	// Fill in this routine
	///First Method img into acc
	CVector3 vector;
	CVector3 resultVector;
	CTransform3x3 inverseM = M.Inverse();
	for (int x = 0; x < acc.Shape().width; x++)
	{
		for (int y = 0; y < acc.Shape().height; y++)
		{
			vector = CVector3::CVector3(x, y, 1);
			resultVector = inverseM * vector;
			if(resultVector[2] == 0) continue;
			resultVector[0] = (int)(resultVector[0] / resultVector[2]);
			resultVector[1] = (int)(resultVector[1] / resultVector[2]);
			if ((resultVector[0] >= 0) && (resultVector[0] <= img.Shape().width) && (resultVector[1] >= 0) && (resultVector[1] <= img.Shape().height))
			{						
				//int bound = min(min(resultVector[0], img.Shape().width - resultVector[0]), min(resultVector[1], img.Shape().height -resultVector[1]));
				int bound = min(resultVector[0], img.Shape().width - resultVector[0]);
				double weight = 0;
				if (bound >= blendWidth)
				{
					weight = 1;
				}
				else
				{
					weight = bound / blendWidth;
				}
				for (int i = 0; i < 3; i++){
						acc.Pixel(x, y, i) += weight * img.Pixel(resultVector[0], resultVector[1], i);
				}
				acc.Pixel(x, y, 3) += weight;
				//cout<<acc.Pixel(x, y, 3);
			}
		}
	}
	// END TODO
}



/******************* TO DO *********************
* NormalizeBlend:
*	INPUT:
*		acc: input image whose alpha channel (4th channel) contains
*		     normalizing weight values
*		img: where output image will be stored
*	OUTPUT:
*		normalize r,g,b values (first 3 channels) of acc and store it into img
*/
	static void NormalizeBlend(CFloatImage& acc, CByteImage& img)
	{
		// BEGIN TODO
		// fill in this routine..	
		int w = acc.Shape().height;
		int h = acc.Shape().width;
		for (int y = 0; y < w; y++) {
			for (int x = 0; x < h; x++) {
				for (int z = 0; z < 3; z++)
				{
					if (acc.Pixel(x, y, 3) != 0)
						img.Pixel(x, y, z) = acc.Pixel(x, y, z) / acc.Pixel(x, y, 3);
					else
						img.Pixel(x, y, z) = 0;
				}
				img.Pixel(x, y, 3) = 1;
			}
		}
	}

	/******************* TO DO *********************
	* BlendImages:
	*	INPUT:
	*		ipv: list of input images and their relative positions in the mosaic
	*		blendWidth: width of the blending function
	*	OUTPUT:
	*		create & return final mosaic by blending all images
	*		and correcting for any vertical drift
	*/
	CByteImage BlendImages(CImagePositionV& ipv, float blendWidth)
	{
		// Assume all the images are of the same shape (for now)
		CByteImage& img0 = ipv[0].img;
		CShape sh        = img0.Shape();
		int width        = sh.width;
		int height       = sh.height;
		int nBands       = sh.nBands;

		int n = ipv.size();
		if (n == 0) return CByteImage(0,0,1);

		bool is360 = false;

		if (ipv[0].imgName == ipv[n-1].imgName)
			is360 = true;

		// Compute the bounding box for the mosaic
		float min_x = FLT_MAX, min_y = FLT_MAX;
		float max_x = 0, max_y = 0;
		int i;
		for (i = 0; i < n; i++)
		{
			CTransform3x3 &T = ipv[i].position;

			// BEGIN TODO
			// add some code here to update min_x, ..., max_y

			CVector3 corners[4];

			corners[0][0] = 0.0;
			corners[0][1] = 0.0;
			corners[0][2] = 1.0;

			corners[1][0] = width - 1;
			corners[1][1] = 0.0;
			corners[1][2] = 1.0;

			corners[2][0] = 0.0;
			corners[2][1] = height - 1;
			corners[2][2] = 1.0;

			corners[3][0] = width - 1;
			corners[3][1] = height - 1;
			corners[3][2] = 1.0;

			for(int i = 0; i < 4; i ++)
				corners[i] = T * corners[i];

			for(int i = 0; i < 4; i ++) {
				corners[i][0] /= corners[i][2];
				corners[i][1] /= corners[i][2];
			}

			for(int i = 0; i < 4; i ++) {
				min_x = float(MIN(min_x, corners[i][0]));
				max_x = float(MAX(max_x, corners[i][0]));
				min_y = float(MIN(min_y, corners[i][1]));
				max_y = float(MAX(max_y, corners[i][1]));
			}

			// END TODO
		}

		// Create a floating point accumulation image
		CShape mShape((int)(ceil(max_x) - floor(min_x)),
			(int)(ceil(max_y) - floor(min_y)), nBands + 1);
		CFloatImage accumulator(mShape);
		accumulator.ClearPixels();

		double x_init, x_final;
		double y_init, y_final;
		// Add in all of the images
		for (i = 0; i < n; i++) {
			// Compute the sub-image involved
			CTransform3x3 &M = ipv[i].position;
			CTransform3x3 M_t = CTransform3x3::Translation(-min_x, -min_y) * M;
			CByteImage& img = ipv[i].img;
			// Perform the accumulation
			AccumulateBlend(img, accumulator, M_t, blendWidth);

			if (i == 0) {
				CVector3 p;
				p[0] = 0.5 * width;
				p[1] = 0.0;
				p[2] = 1.0;

				p = M_t * p;
				x_init = p[0];
				y_init = p[1];
			} else if (i == n - 1) {
				CVector3 p;
				p[0] = 0.5 * width;
				p[1] = 0.0;
				p[2] = 1.0;

				p = M_t * p;
				x_final = p[0];
				y_final = p[1];
			}
		}

		// Normalize the results
		mShape = CShape((int)(ceil(max_x) - floor(min_x)),
			(int)(ceil(max_y) - floor(min_y)), nBands);

		CByteImage compImage(mShape);
		NormalizeBlend(accumulator, compImage);
		bool debug_comp = false;
		if (debug_comp)
			WriteFile(compImage, "tmp_comp.tga");

		// Allocate the final image shape
		int outputWidth;

		bool crop = false;  // set to true to crop
		if (crop) {
			outputWidth = mShape.width - width;
		} else {
			outputWidth = mShape.width;
		}

		CShape cShape(outputWidth, mShape.height, nBands);

		CByteImage croppedImage(cShape);

		// Compute the affine translations
		CTransform3x3 A;

		// BEGIN TODO
		// fill in appropriate entries in A to trim the left edge and
		// to take out the vertical drift if this is a 360 panorama
		// (i.e. is360 is true)

		if(is360) {
			// x translation
			A[0][0] = 1;
			A[0][1] = 0;
			A[0][2] = (width - max_x)/2; 

			// y translation
			A[1][0] = -ipv[n-1].position[1][2] / cShape.width;
			//A[1][0] = 0;
			A[1][1] = 1;
			A[1][2] = 0;

			A[2][0] = 0;
			A[2][1] = 0;
			A[2][2] = 1;
		}


		// END TODO

		// Warp and crop the composite
		WarpGlobal(compImage, croppedImage, A, eWarpInterpLinear);

		return croppedImage;
	}
